//头文件
#include "geometry_msgs/msg/twist.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#include "sentry_interfaces/msg/robot_hp.hpp"

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "behaviortree_cpp/behavior_tree.h"
#include "behaviortree_cpp/bt_factory.h"
using std::placeholders::_1;
using namespace std::chrono_literals;
#ifndef RM_BT_H
#define RM_BT_H

//值的定义
std::string start_op_value="";
geometry_msgs::msg::Twist cmd_gimbal_value;
std_msgs::msg::Float32MultiArray robot_theta_value;
sentry_interfaces::msg::RobotHP robot_hp_value;
std::string friend_or_foe_value="";
std::string robot_id_value="";
std::string hp_min_value="";
std::string hp_max_value="";
std::string goal_pose_bt_value="0.0;0.0";
std::string goal_y_value="";
std::string goal_x_value="";

//创建ros对行为树的修饰器
class ROS2DecoratorNode  : public BT::SyncActionNode
{
public:
  ROS2DecoratorNode(const std::string& name, const BT::NodeConfig& config) :
    BT::SyncActionNode(name, config)
  {
    robot_theta_value.data.push_back(0.0);
  }

  BT::NodeStatus tick() override
  {
    //端口映射
    getInput("start_op", start_op_value);
    getInput("cmd_gimbal", cmd_gimbal_value);
    setOutput("robot_theta", robot_theta_value);
    setOutput("robot_hp", robot_hp_value);
    getInput("goal_pose_bt", goal_pose_bt_value);
    

    return BT::NodeStatus::SUCCESS;
  }


  static BT::PortsList providedPorts()
  {
    
    return {
      //端口注册
    BT::InputPort<std::string>("start_op"),
    BT::InputPort<geometry_msgs::msg::Twist>("cmd_gimbal"),
    BT::OutputPort<std_msgs::msg::Float32MultiArray>("robot_theta"),
    BT::OutputPort<sentry_interfaces::msg::RobotHP>("robot_hp"),
    BT::InputPort<std::string>("goal_pose_bt"),

    };
  }
};


class RM_bt : public rclcpp::Node 
{
  public:
    RM_bt():Node("RM_bt") 
    {
    RCLCPP_INFO(this->get_logger(), "RM_bt,go");
    //创建订阅方

    subscription_robot_theta =this->create_subscription<std_msgs::msg::Float32MultiArray>("robot_theta", 10,std::bind(&RM_bt::topic_callback_robot_theta, this, std::placeholders::_1));


    subscription_robot_hp =this->create_subscription<sentry_interfaces::msg::RobotHP>("robot_hp", 10,std::bind(&RM_bt::topic_callback_robot_hp, this, std::placeholders::_1));


    //创建发布方

    publisher_start_op = this->create_publisher<std_msgs::msg::String>("start_op", 10);
    timer_start_op = this->create_wall_timer(500ms, std::bind(&RM_bt::timer_callback_start_op, this));


    publisher_cmd_gimbal = this->create_publisher<geometry_msgs::msg::Twist>("cmd_gimbal", 10);
    timer_cmd_gimbal = this->create_wall_timer(500ms, std::bind(&RM_bt::timer_callback_cmd_gimbal, this));


    publisher_goal_pose_bt = this->create_publisher<std_msgs::msg::String>("goal_pose_bt", 10);
    timer_goal_pose_bt = this->create_wall_timer(500ms, std::bind(&RM_bt::timer_callback_goal_pose_bt, this));


    }
    //回调函数

    void topic_callback_robot_theta(const std_msgs::msg::Float32MultiArray & msg)
    {
      robot_theta_value=msg;

    }


    void topic_callback_robot_hp(const sentry_interfaces::msg::RobotHP & msg)
    {
      robot_hp_value=msg;
    }


    //定时器

    void timer_callback_start_op()
    {
      auto message = std_msgs::msg::String();
      message.data = start_op_value;
      publisher_start_op->publish(message);
    }


    void timer_callback_cmd_gimbal()
    {
      auto message = geometry_msgs::msg::Twist();
      message = cmd_gimbal_value;
      publisher_cmd_gimbal->publish(message);
    }


    void timer_callback_goal_pose_bt()
    {
      auto message = std_msgs::msg::String();
      message.data = goal_pose_bt_value;
      publisher_goal_pose_bt->publish(message);
    }


  private:
    //初始化订阅方

    rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr subscription_robot_theta;


    rclcpp::Subscription<sentry_interfaces::msg::RobotHP>::SharedPtr subscription_robot_hp;

   
    //初始化发布方

    rclcpp::TimerBase::SharedPtr timer_start_op;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_start_op;


    rclcpp::TimerBase::SharedPtr timer_cmd_gimbal;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_cmd_gimbal;


    rclcpp::TimerBase::SharedPtr timer_goal_pose_bt;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_goal_pose_bt;

};

#endif